/***************************************************************************
 *  Filename: gpsreader.cpp
 *  Class: GPSReader
 *
 *  Copyright 2012  Jacco van der Spek
 *  <jaccovdspek@gmail.com>
 *
 *  Copyright 2012  Mario Henrique Voorsluys
 *  <forcaeluz@gmail.com>
 *
 ****************************************************************************/

/*
 *
 * This file is part of %ProjectName%.
 *
 *    %ProjectName% is free software: you can redistribute it and/or modify
 *    it under the terms of the GNU General Public License as published by
 *    the Free Software Foundation, either version 3 of the License, or
 *    (at your option) any later version.
 *
 *    %ProjectName% is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *    GNU General Public License for more details.
 *
 *    You should have received a copy of the GNU General Public License
 *    along with %ProjectName%.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "gpshandler.h"
#include <QDebug>

GPSHandler::GPSHandler(QString uartPort, QObject *parent) :
    QObject(parent), gpsdatareceiver(false,0)
{
    QString rmc = "$PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29\r\n";
    QString tenhz = "$PMTK300,100,0,0,0,0*2C\r\n";
    gpsdatareceiver.setupPort(uartPort,BAUD9600,DATA_8,PAR_NONE,FLOW_OFF,STOP_1);
    setupGPS(rmc, tenhz);
    connect(&gpsdatareceiver, SIGNAL(newLineReady()), this, SLOT(receiveGPSData()));

}

QString GPSHandler::checkSumCalculation(QString cleaninput)
{
    QByteArray cli = cleaninput.toLocal8Bit();
    char c = 0;

    for(int i = 0; i < cli.length(); i++) //XOR whole sentence
    {
        char a = cli.at(i);
        c = c^a;
    }
    //Added an int caster to remove ambiguity. (Char and int are ambiguous according to ISO C++)
    QString result = QString("%1").arg((int)c,0,16);

    return result;
}

void GPSHandler::handleInput(QString rawdata)
{
    qDebug() << "[GPSHandler] Started handling input of" << rawdata;
    if(gpslist.size()>=100){
        GPSData *tmp = gpslist.takeFirst();
        delete(tmp);
    }

    if(rawdata.left(6)!="$GPRMC")
    {
        qWarning() << "[GPSHandler] No GPRMC message: " << rawdata;
    }
    else
    {

        QStringList splitinput = rawdata.split("*"); // split for calculating checksum
        if(splitinput.size() <= 1){ // If the string is not complete, leave.
            qWarning() << "[GPSHandler] Invalid input string, no *";
            return;
        }
        QString firstpart = splitinput.at(0);
        QString cleaninput = firstpart.remove("$");
        QString checksum = splitinput.at(1);
        QString calculatedchecksum = checkSumCalculation(cleaninput); //calculate checksum over the characters between $ and *

        if(calculatedchecksum.toInt()==checksum.toInt())    //check if calculated checksum is send checksum
        {


            QStringList parts = rawdata.split(","); //split for defining all variables
            if(parts.size() <= 1){
                qWarning() << "[GPSHandler] Invalid input string, not enough fields.";
                return;
            }
            GPSData *newGpsData = new GPSData();
            QString sts = parts.at(2);
            if(!sts.isEmpty()){
                newGpsData->setStatus(sts.at(0));
            }
            if(!parts.at(3).isEmpty()){
                newGpsData->setLatitude(parseLatitude(parts.at(3))); //to google maps coords
            }
            QString noso = parts.at(4);
            if(!noso.isEmpty()){
                newGpsData->setNS(noso.at(0));
            }
            if(!parts.at(5).isEmpty()){
                newGpsData->setLongitude(parseLongitude(parts.at(5)));
            }
            QString eawe = parts.at(6);
            if(!eawe.isEmpty()){
                newGpsData->setEW(eawe.at(0));
            }
            if(!parts.at(7).isEmpty()){
                newGpsData->setVelocity(parts.at(7).toDouble());
            }
            if(!parts.at(8).isEmpty())
            {
                newGpsData->setBearing(parts.at(7).toDouble());
            }

            gpslist.append(newGpsData);
        }
        else
        {
            qWarning() << "Checksum failed";
        }
    }
}

GPSData* GPSHandler::calcCoordTarget(double startlat, double startlong, double bearing, double distance)
{

    qreal R = 6372795 ;//earth's radius in m
    qreal nlat = qAsin(qSin(startlat)*qCos(distance/R)+qCos(startlat)*qSin(distance/R)*qCos(bearing*M_PI/180));
    qreal nlong = startlong+qAtan2(qSin(bearing*M_PI/180)*qSin(distance/R)*qCos(startlat), qCos(distance/R)-qSin(startlat)*qSin(nlat));

    nlat = nlat*180/M_PI; //output in degrees
    nlong = nlong*180/M_PI; //output in degrees

    float newlat = nlat;
    float newlong = nlong;

    GPSData* tmp = new GPSData();
    tmp->setLatitude(newlat);
    tmp->setLongitude(newlong);
    return tmp;
}

void GPSHandler::setupGPS(QString rmc, QString tenhz)
{
    QByteArray tmp;
    tmp.append(rmc);
    gpsdatareceiver.sendToPort(tmp); //only send GPRMC format
    tmp.clear();
    tmp.append(tenhz);
    gpsdatareceiver.sendToPort(tmp); //set update rate to 10 Hz
}

QString GPSHandler::removeLeadingzeros(QString s)
{
    int i = 0;
    while(i < s.length() && s[i]=='0')
    {
        i++;
    }
    s.remove(0,i);
    return s;
}

/** Deprecated **/
QList<GPSData *> GPSHandler::getList(){
    return gpslist;
}

/**
  Pay attention to the fact that the pointer is lost by this handler, and should be deleted by the new owner.
  **/
GPSData* GPSHandler::getBufferedGPSData(){
    if(gpslist.isEmpty()){
        return new GPSData();
    }
    return gpslist.takeFirst();
}

float GPSHandler::parseLatitude(QString latitude) //convert NMEA protocol to google maps coords
{
    QString deg = removeLeadingzeros(latitude.left(2));
    QString min = removeLeadingzeros(latitude.right(7));
    float lat = deg.toFloat()+min.toFloat()/60;
    return lat;
}

float GPSHandler::parseLongitude(QString longitude)
{
    QString deg = removeLeadingzeros(longitude.left(3));
    QString min = removeLeadingzeros(longitude.right(7));
    float lng = deg.toFloat()+min.toFloat()/60;
    return lng;
}

void GPSHandler::receiveGPSData()
{
    QString msg = gpsdatareceiver.readLine();
    msg.chop(1);
    handleInput(msg);

}
